package robot;


import java.io.FileNotFoundException;
import java.util.List;

import javax.microedition.lcdui.Image;

import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;

public class Robot{
	/** Left Motor of NxTRover**/
	public boolean isForward = false; //whether or not the robot is moving forward already
    public TouchSensor touch = new TouchSensor(SensorPort.S1);

	private NXTRegulatedMotor left;

	/** Right Motor of NxTRover **/
	private NXTRegulatedMotor right;
	
	private NXTRegulatedMotor arm;
	/** Time/Count per degree it takes for the wheel to turn by a degree **/ 
	//private double countPerDegree = 
	//private double wheelDiameter = 5;
	private Camera cam;
	
	private RobotBehaviour strategy;
	
	private DifferentialPilot pilot;
	
	double turnRate = 100;

	/** Constructer: Initialize the Motors, and Count per Degree & sets default speed**/ 
	public Robot(){

		//Initialize Motors
		left = Motor.B;
		right = Motor.A;
		arm = Motor.C;


		//set default speed
		left.setSpeed(50);
		right.setSpeed(50);
		
		// DifferentialPilot is our motion controller, we will exclusively use the object to navigate the robot around
	    // the first two arguments are wheel diameters and track width (in cm) respectively
	    // last two arguments are left and right motors respectively
	    pilot = new DifferentialPilot(2.5f, 5.5f, left, right); //check wheel diameter
		
		// Calculate counts per degree
		//double wheelCircumference = wheelDiameter * Math.PI;
		//double fullRotation = (driveLength * Math.PI);
		//countPerDegree = ((fullRotation/wheelCircumference)*ratio); 

	}

	public boolean execute(int command){
		String thecommand = Integer.toString(command);
		if (thecommand.equals("111000")) //Hold forward
		{
			halt();
			moveForward(0); //0 means no distance specified
		}
		
		if (thecommand.startsWith("101")) //Move Forward X distance
		{
			String forwardString = thecommand.substring(3,6);
			int forwardCommand = Integer.parseInt(forwardString);
			moveForward(forwardCommand);
		}
		
		if (thecommand.equals("112000")) //Hold backward
		{
			halt();
			moveBackward();
		}
		if (thecommand.startsWith("102")) //Move Backward X distance
		{
			String backwardString = thecommand.substring(3,6);
			int backwardCommand = Integer.parseInt(backwardString);
			moveBackwardX(backwardCommand);
		}
		if (thecommand.equals("222000")) //Hold left 90 degree turn 
		{
			turnLeft(90);
		}
		
		if (thecommand.startsWith("202")) //turn  Left X degrees
		{
			String leftString = thecommand.substring(3, 6);	
			int leftCommand = Integer.parseInt(leftString);
			turnLeft(leftCommand);
		}
		
		if (thecommand.equals("211000"))  //Hold right 90 degree turn
		{
			turnRight(90);
		}
		if (thecommand.startsWith("201")) //turn  Right X degrees
		{
			String rightString = thecommand.substring(3, 6);	
			int rightCommand = Integer.parseInt(rightString);
			turnRight(rightCommand);
		}
		if (thecommand.equals("322000")) //Hold arm Left
		{
			armLeft();
		}
		if (thecommand.equals("311000"))
		{
			armRight();
		}
		
		if (thecommand.equals("255000")) //Stop
		{
			halt();
		}
		
		if (thecommand.startsWith("701")) //Set Speed X
		{
			setSpeedX(thecommand);
		}
		
		return true;
	}

	/*private void moveForwardX(int forwardCommand) {
		isForward = true; 
		pilot.travel((double)forwardCommand);
		
	}*/
	
	private void moveBackwardX(int backwardCommand){
		pilot.travel(-(double)backwardCommand);
	}

	private void setSpeedX(String thecommand)
	{
		String speedString = thecommand.substring(3, 6);
		int speedCommand = Integer.parseInt(speedString);
		
		left.setSpeed(speedCommand);
		right.setSpeed(speedCommand);
		
	}

	private void turnLeft(int i) {
		if(isForward){
			//we should curve..
			curveLeft(i);
		}else{
			//in place rotation
			pilot.rotate(-i);
	
		}		
	}
	
	private void turnRight(int i){
		if(isForward){
			//we should curve..
			curveRight(i);
		}else{
			//in place rotation
			pilot.rotate(i);
		}
	}

	private void armRight() 
	{
		arm.forward();
	}

	private void armLeft() {
		
		arm.backward();
	}

	public void setBehaviour(RobotBehaviour b){
		strategy=b;	
	}
	
	private List<Image> scan(){
		return null;	
	}

	private void moveForward(int distance){
		
		//we're moving forward..
		isForward = true; 

		if(distance==0){
		left.forward();
		right.forward();
		}else{
			pilot.travel((double)distance);
		}
		
	}

	private void moveBackward(){
		left.backward();
		right.backward();
	} 

	/** stops the motors left and right of the NxtRovers to come to complete halt */
	public void halt(){
		left.stop();
		right.stop();   
		arm.stop();
		isForward = false; 

	}

	private void curveLeft(int angle)
	{
		pilot.steer(turnRate, angle); //turn rate, angel
	}
	
	private void curveRight(int angle)
	{
		pilot.steer(-turnRate, angle);
	}
	
	//stops the the rover smoothly (not suddent break)
	private void coast(){
		left.flt();
		right.flt();   
	}


	/** Change the current speed of the NxtRover to input parameter speed, which should be a integer number
	 * between 0 and 100. * */
	private void changeRoverSpeed(int speed){
		if(speed > 0){
			left.setSpeed(speed);
			right.setSpeed(speed);
		}
		else{
			left.setSpeed(50);
			right.setSpeed(50);
		}
	}

	/** Turn the robot for the given number of milliseconds. This method is the basis of the
	 * turnLeft() and turnRight() methods */

	private void turn(int milliseconds) {

		try {
			if(milliseconds < 0) {

				left.forward();
				right.backward();
				milliseconds = -milliseconds;
			} 
			else {

				right.forward();
				left.backward();
			}
			Thread.sleep(milliseconds);
			halt();
		} 
		catch (InterruptedException e) {}
	}
//
//	/** Turn towards the left (counter-clockwise) by degrees, or clockwise if negative. */
//
//	private void turnLeft(double degrees) {
//		int count = (int)(countPerDegree * degrees);
//		turn(count);
//	}
//
//	/** Turn towards the right (clockwise) by degrees, or counter-clockwise if negative. */
//	private void turnRight(double degrees) {
//		int count = (int)(countPerDegree * degrees);
//		turn(-count);
//	}


	/** This method is basis for the methods moveForward(int milliseconds) and moveBackward(int milliseconds) * */
	private void move(int milliseconds)
			throws InterruptedException {
		if( milliseconds >= 0 ) {

			left.forward();
			right.forward();
		} else {

			left.backward();
			right.backward();
			milliseconds = -milliseconds;
		}
		Thread.sleep(milliseconds);
		halt();
	}

	/** Move forward for approximately milliseconds * */
	/*private void moveForward(int milliseconds) {
		try {
			move(milliseconds);

		} catch (InterruptedException e) {}
	}*/

	/** Move backward for approximately milliseconds*/
	private void moveBackward(int milliseconds) {
		try {
			move(-milliseconds);

		} catch (InterruptedException e) {}
	}

	public void executeListofCommands(String fileName) throws FileNotFoundException
	{


	}
}